# Software License Agreement (BSD License)
#
# Copyright (c) 2012, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

from __future__ import annotations

import array
import math
import re
import sys
from base64 import standard_b64decode, standard_b64encode
from collections.abc import Sequence
from typing import TYPE_CHECKING, Any

import numpy as np
from builtin_interfaces.msg import Duration as DurationMsg
from builtin_interfaces.msg import Time as TimeMsg
from rclpy.clock import ROSClock
from rclpy.parameter import Parameter
from std_msgs.msg import Header as HeaderMsg

from rosbridge_library.internal import ros_loader
from rosbridge_library.internal.type_support import ROSMessage
from rosbridge_library.util import bson

if TYPE_CHECKING:
    from collections.abc import Callable

    from rclpy.clock import Clock
    from rclpy.node import Node


type_map = {
    "bool": ("bool", "boolean"),
    "int": (
        "int8",
        "octet",
        "uint8",
        "char",
        "int16",
        "uint16",
        "int32",
        "uint32",
        "int64",
        "uint64",
    ),
    "float": ("float32", "float64", "double", "float"),
    "str": ("string"),
}
primitive_types = (bool, int, float, str)
list_types = (list, tuple, np.ndarray, array.array)

# Type aliases for static type checking
PrimitiveType = bool | int | float | str
ListType = list | tuple | np.ndarray | array.array

ros_time_types = ("builtin_interfaces/Time", "builtin_interfaces/Duration")
ros_primitive_types = (
    "bool",
    "boolean",
    "octet",
    "char",
    "int8",
    "uint8",
    "int16",
    "uint16",
    "int32",
    "uint32",
    "int64",
    "uint64",
    "float32",
    "float64",
    "float",
    "double",
    "string",
)
ros_header_types = ("Header", "std_msgs/Header", "roslib/Header")
ros_binary_types = ("uint8[]", "char[]", "sequence<uint8>", "sequence<char>")
# Remove the list type wrapper, and length specifier, from rostypes i.e. sequence<double, 3>
list_tokens = re.compile(r"<(.+?)(, \d+)?>")
bounded_array_tokens = re.compile(r"(.+)\[.*\]")
ros_binary_types_list_braces = (
    ("uint8[]", re.compile(r"uint8\[[^\]]*\]")),
    ("char[]", re.compile(r"char\[[^\]]*\]")),
)

binary_encoder = None
binary_encoder_type = "default"
bson_only_mode = False


# TODO(@jubeira): configure module with a node handle.
# The original code doesn't seem to actually use these parameters.
def configure(node_handle: Node | None = None) -> None:
    global binary_encoder, binary_encoder_type, bson_only_mode

    if node_handle is not None:
        binary_encoder_type = (
            node_handle.get_parameter_or("binary_encoder", Parameter("", value="default"))
            .get_parameter_value()
            .string_value
        )
        bson_only_mode = (
            node_handle.get_parameter_or("bson_only_mode", Parameter("", value=False))
            .get_parameter_value()
            .bool_value
        )

    if binary_encoder is None:
        if binary_encoder_type == "bson" or bson_only_mode:
            binary_encoder = bson.Binary
        elif binary_encoder_type in {"default", "b64"}:
            binary_encoder = standard_b64encode
        else:
            print(f"Unknown encoder type '{binary_encoder_type}'")
            sys.exit(0)


def get_encoder() -> Callable[[ListType], bytes]:
    configure()
    assert binary_encoder is not None, "Binary encoder is not configured"
    return binary_encoder


class InvalidMessageException(Exception):
    def __init__(self, inst: Any) -> None:  # noqa: ANN401
        Exception.__init__(
            self,
            f"Unable to extract message values from {type(inst).__name__} instance",
        )


class NonexistentFieldException(Exception):
    def __init__(self, basetype: str, fields: list[str]) -> None:
        Exception.__init__(
            self,
            "Message type {} does not have a field {}".format(basetype, ".".join(fields)),
        )


class FieldTypeMismatchException(Exception):
    def __init__(
        self, roottype: str, fields: list[str], expected_type: str, found_type: type
    ) -> None:
        if roottype == expected_type:
            Exception.__init__(
                self,
                f"Expected a JSON object for type {roottype} but received a {found_type}",
            )
        else:
            Exception.__init__(
                self,
                "{} message requires a {} for field {}, but got a {}".format(
                    roottype, expected_type, ".".join(fields), found_type
                ),
            )


def extract_values(inst: ROSMessage) -> dict[str, Any]:
    rostype = msg_instance_type_repr(inst)
    if rostype is None:
        raise InvalidMessageException(inst=inst)
    return _from_object_inst(inst, rostype)


def populate_instance(
    msg: dict[str, Any], inst: ROSMessage, clock: Clock | None = None
) -> ROSMessage:
    """
    Populate a ROS message instance with the provided values.

    Return an instance of the provided class, with its fields populated
    according to the values in msg.
    """
    if clock is None:
        clock = ROSClock()

    inst_type = msg_instance_type_repr(inst)

    return _to_object_inst(msg, inst_type, inst_type, clock, inst, [])


def msg_instance_type_repr(msg_inst: ROSMessage) -> str:
    """Return a string representation of a ROS2 message type from a message instance."""
    # Message representation: '{package}.msg.{message_name}({fields})'.
    # A representation like '_type' member in ROS1 messages is needed: '{package}/{message_name}'.
    # E.g: 'std_msgs/Header'
    msg_type = type(msg_inst)
    if msg_type in primitive_types or msg_type in list_types:
        return str(type(msg_inst))
    inst_repr = str(msg_inst).split(".")
    return "{}/{}".format(inst_repr[0], inst_repr[2].split("(")[0])


def msg_class_type_repr(msg_class: type[ROSMessage]) -> str:
    """Return a string representation of a ROS2 message type from a class representation."""
    # The string representation of the class is <class '{package}.msg._{message}.{Message}'>
    # (e.g. <class 'std_msgs.msg._string.String'>).
    # This has to be converted to {package}/msg/{Message} (e.g. std_msgs/msg/String).
    class_repr = str(msg_class).split("'")[1].split(".")
    return f"{class_repr[0]}/{class_repr[1]}/{class_repr[3]}"


def _from_inst(
    inst: ROSMessage | ListType | PrimitiveType | bytes, rostype: str
) -> dict | list | PrimitiveType | bytes | None:
    # Special case for uint8[], we encode the string
    for binary_type, expression in ros_binary_types_list_braces:
        if expression.sub(binary_type, rostype) in ros_binary_types:
            if not isinstance(inst, list_types):
                err_msg = f"inst is not a list type, but a {type(inst)}"
                raise TypeError(err_msg)
            encoded = get_encoder()(inst)
            return encoded.decode("ascii")

    # Check for time or duration
    if rostype in ros_time_types:
        if not isinstance(inst, TimeMsg | DurationMsg):
            err_msg = f"inst is not TimeMsg or DurationMsg, but a {type(inst)}"
            raise TypeError(err_msg)
        return {"sec": inst.sec, "nanosec": inst.nanosec}

    # Check for primitive types
    if rostype in ros_primitive_types:
        if not isinstance(inst, (*primitive_types, bytes)):
            err_msg = f"inst is not a primitive type or bytes, but a {type(inst)}"
            raise TypeError(err_msg)
        return _from_primitive_inst(inst, rostype)

    # Check if it's a list or tuple
    if isinstance(inst, list_types):
        return _from_list_inst(inst, rostype)

    # Assume it's otherwise a full ros msg object
    if not isinstance(inst, ROSMessage):
        err_msg = f"inst is not a ROS Message, but a {type(inst)}"
        raise TypeError(err_msg)
    return _from_object_inst(inst, rostype)


def _from_primitive_inst(inst: PrimitiveType | bytes, rostype: str) -> PrimitiveType | bytes | None:
    if bson_only_mode:
        return inst

    # JSON does not support Inf and NaN. They are mapped to None and encoded as null
    if rostype in type_map["float"]:
        if not isinstance(inst, float):
            err_msg = f"inst is not a float, but a {type(inst)}"
            raise TypeError(err_msg)
        if math.isnan(inst) or math.isinf(inst):
            return None

    # octet is translated to byte array with length 1
    # JSON does not support byte array. They are converted to int
    if rostype == "octet":
        if not isinstance(inst, bytes):
            err_msg = f"inst is not bytes, but a {type(inst)}"
            raise TypeError(err_msg)
        return int.from_bytes(inst, "little")

    return inst


def _from_list_inst(inst: ListType, rostype: str) -> list:
    # Can duck out early if the list is empty
    if len(inst) == 0:
        return []

    # Remove the list indicators from the rostype
    base_rostype = _remove_list_indicators(rostype)

    # Shortcut for primitives
    if base_rostype in ros_primitive_types:
        # Convert to Built-in integer types to dump as JSON
        if isinstance(inst, np.ndarray) and (
            base_rostype in type_map["int"] or base_rostype in type_map["float"]
        ):
            return inst.tolist()

        if base_rostype not in type_map["float"]:
            return list(inst)

    # Call to _to_inst for every element of the list
    return [_from_inst(x, base_rostype) for x in inst]


def _from_object_inst(inst: ROSMessage, _rostype: str) -> dict:
    # Create an empty dict then populate with values from the inst
    msg = {}
    # Equivalent for zip(inst.__slots__, inst._slot_types) in ROS1:
    for field_name, field_rostype in inst.get_fields_and_field_types().items():
        field_inst = getattr(inst, field_name)
        msg[field_name] = _from_inst(field_inst, field_rostype)
    return msg


def _to_inst(
    msg: dict[str, Any] | Sequence | PrimitiveType,
    rostype: str,
    roottype: str,
    clock: Clock | None = None,
    inst: object | None = None,
    stack: list[str] | None = None,
) -> object:
    if clock is None:
        clock = ROSClock()
    if stack is None:
        stack = []

    # Check if it's uint8[], and if it's a string, try to b64decode
    for binary_type, expression in ros_binary_types_list_braces:
        if expression.sub(binary_type, rostype) in ros_binary_types:
            if not isinstance(msg, Sequence):
                err_msg = f"msg is not a Sequence, but a {type(msg)}"
                raise TypeError(err_msg)
            return _to_binary_inst(msg)

    # Check the type for time or rostime
    if rostype in ros_time_types:
        if not isinstance(msg, dict | str):
            err_msg = f"msg is not dict or str, but a {type(msg)}"
            raise TypeError(err_msg)
        if inst is not None and not isinstance(inst, TimeMsg | DurationMsg):
            err_msg = f"inst is not Time or Duration message, but a {type(inst)}"
            raise TypeError(err_msg)
        return _to_time_inst(msg, rostype, clock, inst)

    # Check to see whether this is a primitive type
    if rostype in ros_primitive_types:
        if not isinstance(msg, primitive_types):
            err_msg = f"msg is not a primitive type, but a {type(msg)}"
            raise TypeError(err_msg)
        return _to_primitive_inst(msg, rostype, roottype, stack)

    # Check whether we're dealing with a list type
    if inst is not None and isinstance(inst, list_types):
        if not isinstance(msg, list_types):
            err_msg = f"msg is not a list type, but a {type(msg)}"
            raise TypeError(err_msg)
        return _to_list_inst(msg, rostype, roottype, clock, inst, stack)

    # Otherwise, the type has to be a full ros msg type, so msg must be a dict
    if inst is None:
        inst = ros_loader.get_message_instance(rostype)

    if not isinstance(msg, dict):
        err_msg = f"msg is not a dict, but a {type(msg)}"
        raise TypeError(err_msg)
    if not isinstance(inst, ROSMessage):
        err_msg = f"inst is not a ROS Message, but a {type(inst)}"
        raise TypeError(err_msg)
    return _to_object_inst(msg, rostype, roottype, clock, inst, stack)


def _to_binary_inst(msg: Sequence) -> list | bytes | array.array:
    if isinstance(msg, str):
        return list(standard_b64decode(msg))
    if isinstance(msg, list):
        return msg
    if isinstance(msg, bytes):
        # Using the frombytes() method with a memoryview of the data allows for zero copying of
        # data thanks to Python's buffer protocol (HUGE time-saver for large arrays)
        data = array.array("B")
        data.frombytes(memoryview(msg))
        return data
    return bytes(bytearray(msg))


def _to_time_inst(
    msg: dict[str, Any] | str,
    rostype: str,
    clock: Clock,
    inst: TimeMsg | DurationMsg | None = None,
) -> TimeMsg | DurationMsg:
    # A special case for the string "now"
    if isinstance(msg, str):
        if rostype == "builtin_interfaces/Time" and msg == "now":
            return clock.now().to_msg()

        err_msg = f"Invalid string for {rostype}: {msg}"
        raise ValueError(err_msg)

    # Create an instance if we haven't been provided with one
    if inst is None:
        if rostype == "builtin_interfaces/Time":
            inst = TimeMsg()
        elif rostype == "builtin_interfaces/Duration":
            inst = DurationMsg()
        else:  # This should never happen, but just in case
            msg = f"Unknown rostype for time: {rostype}"
            raise ValueError(msg)

    # Copy across the fields, try ROS1 and ROS2 fieldnames
    for field in ["sec", "secs"]:
        if field in msg:
            inst.sec = msg[field]
            break
    for field in ["nanosec", "nsecs"]:
        if field in msg:
            inst.nanosec = msg[field]
            break

    return inst


def _to_primitive_inst(
    msg: PrimitiveType, rostype: str, roottype: str, stack: list[str]
) -> PrimitiveType | bytes:
    # Typecheck the msg
    if isinstance(msg, int) and rostype in type_map["float"]:
        # probably wrong parsing,
        # fix that by casting the int to the expected float
        msg = float(msg)

    # Convert to byte
    if rostype == "octet" and isinstance(msg, int):
        return bytes([msg])

    msgtype = type(msg)
    if msgtype in primitive_types and rostype in type_map[msgtype.__name__]:
        return msg
    if isinstance(msg, str) and rostype in type_map[msgtype.__name__]:
        return msg
    raise FieldTypeMismatchException(roottype, stack, rostype, msgtype)


def _to_list_inst(
    msg: ListType,
    rostype: str,
    roottype: str,
    clock: Clock,
    inst: ListType,
    stack: list[str],
) -> ListType:
    # Typecheck the msg
    if not isinstance(msg, list_types):
        raise FieldTypeMismatchException(roottype, stack, rostype, type(msg))

    # Can duck out early if the list is empty
    if len(msg) == 0:
        return []

    # Special mappings for numeric types https://design.ros2.org/articles/idl_interface_definition.html
    if isinstance(inst, array.array):
        del inst[:]
        inst.extend(msg)  # accepts both ints and floats which may come from json
        return inst
    if isinstance(inst, np.ndarray):
        inst[:] = msg  # accepts both ints and floats which may come from json
        return inst

    # Remove the list indicators from the rostype
    base_rostype = _remove_list_indicators(rostype)

    # Call to _to_inst for every element of the list
    return [_to_inst(x, base_rostype, roottype, clock, None, stack) for x in msg]


def _to_object_inst(
    msg: dict[str, Any],
    rostype: str,
    roottype: str,
    clock: Clock,
    inst: ROSMessage,
    stack: list[str],
) -> ROSMessage:
    # Typecheck the msg
    if not isinstance(msg, dict):
        raise FieldTypeMismatchException(roottype, stack, rostype, type(msg))

    # Substitute the correct time if we're an std_msgs/Header
    if rostype in ros_header_types:
        if not isinstance(inst, HeaderMsg):
            err_msg = f"inst is not a HeaderMsg, but a {type(inst)}"
            raise TypeError(err_msg)
        inst.stamp = clock.now().to_msg()

    inst_fields: dict[str, str] = inst.get_fields_and_field_types()
    for field_name, field_value in msg.items():
        # Add this field to the field stack
        field_stack = [*stack, field_name]

        # Raise an exception if the msg contains a bad field
        if field_name not in inst_fields:
            raise NonexistentFieldException(roottype, field_stack)

        field_rostype = inst_fields[field_name]
        field_inst = getattr(inst, field_name)

        field_inst_value = _to_inst(
            field_value, field_rostype, roottype, clock, field_inst, field_stack
        )

        setattr(inst, field_name, field_inst_value)

    return inst


def _remove_list_indicators(rostype: str) -> str:
    """
    Remove list indicators from rostype string.

    Handles patterns like:
    - sequence<double, 3> -> double
    - sequence<string> -> string
    - uint8[] -> uint8
    - char[10] -> char

    :param rostype: The ROS type string that may contain list indicators
    :type rostype: str
    :return: The base type without list indicators
    :rtype: str
    :raises ValueError: If the rostype doesn't match expected list patterns
    """
    # Try sequence pattern first: sequence<type, length> or sequence<type>
    list_match = re.search(list_tokens, rostype)
    if list_match:
        return list_match.group(1)

    # Try bounded array pattern: type[length] or type[]
    bounded_match = re.search(bounded_array_tokens, rostype)
    if bounded_match:
        return bounded_match.group(1)

    # If neither pattern matches, this isn't a valid list type
    err_msg = f"Invalid rostype for list: {rostype}"
    raise ValueError(err_msg)
